import grpc
import threading
from .proto import athena_common_athena_grpc_protos_cyberdog_app_pb2 as cyberdog_app_pb2, athena_common_athena_grpc_protos_cyberdog_app_pb2_grpc as cyberdog_app_pb2_grpc
from .mode import Mode
from .motion import Motion
from .attitude import Attitude
from .action import Action
from .audio import Audio
import app as robot_script

class CyberDog:

    def __init__(self, framework):
        self.framework = framework
        self.cyberdog_grpc = cyberdog_app_pb2_grpc
        self.cyberdog_pb2 = cyberdog_app_pb2
        # 设备IP地址
        self.cyberdog_ip = ""
        # 设备订阅服务
        self.cyberdog_subscribe = None
        # 模式控制
        self.mode = Mode(self)
        # 运动控制
        self.motion = Motion(self)
        # 姿态控制
        self.attitude = Attitude(self)
        # 动作控制
        self.action = Action(self)
        # 媒体控制
        self.audio = Audio(self)
        self.control_message_task = threading.Thread(name="control_message_task", target=self.send_control_message_task)
        self.control_message_task.daemon = True
        self.control_message_task.start()

    # 启动设备订阅服务
    def on_subscribe_start(self, ip):
        self.cyberdog_ip = ip
        channel = grpc.insecure_channel(self.cyberdog_ip + ':50051')
        self.framework.common.socket.log("Wait connect")
        try:
            self.cyberdog_subscribe = self.cyberdog_grpc.CyberdogAppStub(channel)
            self.framework.common.socket.log("Connect success")
        except grpc.FutureTimeoutError:
            self.framework.common.socket.log("Connect error")
            return

    # 发送控制消息任务
    def send_control_message_task(self):
        while True:
            if not self.motion.status:
                continue
            if self.cyberdog_subscribe is not None:
                twist = self.cyberdog_pb2.Twist()
                twist.linear.x = self.motion.linear_x
                twist.linear.y = self.motion.linear_y
                twist.linear.z = self.motion.linear_z
                twist.angular.x = self.attitude.angular_x
                twist.angular.y = self.attitude.angular_y
                twist.angular.z = self.attitude.angular_z
                self.cyberdog_subscribe.sendAppDecision(self.cyberdog_pb2.Decissage(twist=twist))
